
// ------------------------------------------------------------
// Math Library 
// ------------------------------------------------------------
// Chris Stephenson 
// ------------------------------------------------------------
// Enriched by ShizheZhou @ 09-8-11 
// ------------------------------------------------------------
// This library is mostly pieced together from other inseficiant 
// free sources I have found (mostly www.flipcode.com code of the day section)
// All angles in degrees.(radians suck!)
// Everything else is basic.
// 
// For the Matrix classes, transfomations dan be writen like this:
// {
//    Matrix4 m;
//    m.settranslate(1, -2, 3).rotate(40, 1, 0.5, 4).scale(0.5, 2, 1.3);
// }
// Instead of this:
// {
//    Matrix4 m;
//    m.settranslate(1, -2, 3)
//    m.rotate(40, 1, 0.5, 4)
//    m.scale(0.5, 2, 1.3);
// }
// ------------------------------------------------------------

#ifndef MY_MATH_LIB
#define MY_MATH_LIB
#pragma once

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <float.h>
#include <limits>
#include <vector>
#include <assert.h>
#pragma warning(disable : 4305)
#pragma warning(disable : 4244)

#if MATLAB_AVAILABLE
#include "Matlab\include\libSVD.h"  
#pragma  comment( lib,	"libSVD.lib")
#endif
// ------------------------------------------------------------
// constants
// ------------------------------------------------------------
// Floating point number precision
#define DOUBLE_PRECISION 1	// uncomment this if double precision needed

#ifdef DOUBLE_PRECISION
#define FTP double
#define FT_MAX DBL_MAX
#else
#define FTP float
#define FT_MAX FLT_MAX
#endif

// PI & conversions
const FTP ML_PI				= 3.141592653589793238;
const FTP ML_DEG_TO_RAD		= 0.017453292519943295;
const FTP ML_RAD_TO_DEG		= 57.29577951308232286;
const FTP ML_PI_DIV_2		= 1.570796326794896558;
const FTP ML_PI_DIV_4		= 0.785398163397448279;

// tolerance values
const int	ML_INT_HIGH_TOLERANCE	= 9999999;	//std::numeric_limits<int>::max();
const int	ML_INT_LOW_TOLERANCE	= -9999999;	//std::numeric_limits<int>::min();

const float ML_LOW_TOLERANCE		= 0.000001f;
const float ML_TOLERANCE			= 0.000100f;
const float ML_HIGH_TOLERANCE		= 0.010000f;

const double ML_D_LOW_TOLERANCE		= 0.0000000001;
const double ML_D_TOLERANCE			= 0.0000000010;
const double ML_D_HIGH_TOLERANCE	= 0.0000000100;

const double RADIAN_TO_ANGLE  = 57.295779513;

// ------------------------------------------------------------
// basic functions
// ------------------------------------------------------------
//added for close contour detection
inline double distance(double x1, double y1, double x2, double y2) {
	return sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) );
}
inline int minimal(double d1, double d2, double d3, double d4) {
	if (d1 <= d2 && d1 <= d3 && d1 <= d4){return 1;}
	if (d2 <= d1 && d2 <= d3 && d2 <= d4){return 2;}
	if (d3 <= d1 && d3 <= d2 && d3 <= d4){return 3;}
	if (d4 <= d1 && d4 <= d2 && d4 <= d3){return 4;}
	return 0;
}
inline double minimum(double d1, double d2, double d3, double d4) {
	if (d1 <= d2 && d1 <= d3 && d1 <= d4){return d1;}
	if (d2 <= d1 && d2 <= d3 && d2 <= d4){return d2;}
	if (d3 <= d1 && d3 <= d2 && d3 <= d4){return d3;}
	if (d4 <= d1 && d4 <= d2 && d4 <= d3){return d4;}
	return 0;
}
inline int sign(double d) {
	if (d == 0) return 0;
	return ((d>0)?1:-1);
}
inline int dround(double d) {
	return ((int)floor(d + 0.5));
}
inline double area(double P1x, double P1y, double P2x, double P2y) {
	return fabs(P1x-P2x)*(P1y+P2y)*0.5;
}
inline double triangle_area(double Ax, double Ay, double Bx, double By, double Cx, double Cy) {
	return ( fabs(-Bx*Ay + Cx*Ay + Ax*By - Cx*By - Ax*Cy + Bx*Cy)/2 );
}
inline double curvature(double P0x, double P0y, double P1x, double P1y, double P2x, double P2y, double P3x, double P3y) {

	double total = 0, delta, x, xx, y, yy, K;
	double interm[4][2];
	int n = 100;
	double length, u, x1, y1, x2, y2;

	delta = 1.0 / n;

	interm[0][0] = -(P0x) + (3 * P1x) - (3 * P2x) + P3x;
	interm[0][1] = -(P0y) + (3 * P1y) - (3 * P2y) + P3y;
	interm[1][0] = (3 * P0x) - (6 * P1x) + (3 * P2x);
	interm[1][1] = (3 * P0y) - (6 * P1y) + (3 * P2y);
	interm[2][0] = -(3 * P0x) + (3 * P1x);
	interm[2][1] = -(3 * P0y) + (3 * P1y);
	interm[3][0] = P0x;
	interm[3][1] = P0y;

	for (int i = 0; i < n; i++)
	{
		x = 3 * (delta * delta * i * i) * interm[0][0] + 2 * delta * i * interm[1][0] + interm[2][0];
		xx = 6 * delta * i * interm[0][0] + 2 * interm[1][0];
		y = 3 * (delta * delta * i * i) * interm[0][1] + 2 * delta * i * interm[1][1] + interm[2][1];
		yy = 6 * delta * i * interm[0][1] + 2 * interm[1][1];

		if ( (x == 0) && (y == 0) )
			K = 0;
		else
		{
			K = ( fabs(x * yy - xx * y) / pow((fabs(x * x + y * y)), 1.5) );
			K = K * K;
		}

		u = delta*i;
		x1 = interm[0][0] * u * u * u + interm[1][0] * u * u + interm[2][0] * u + interm[3][0];
		y1 = interm[0][1] * u * u * u + interm[1][1] * u * u + interm[2][1] * u + interm[3][1];
		u = delta*(i+1);
		x2 = interm[0][0] * u * u * u + interm[1][0] * u * u + interm[2][0] * u + interm[3][0];
		y2 = interm[0][1] * u * u * u + interm[1][1] * u * u + interm[2][1] * u + interm[3][1];

		length = distance(x1,y1,x2,y2);

		total += K * length;
	}
	return total;
}
inline double angle(double p1x, double p1y, double p2x, double p2y, double p3x, double p3y){
	double p1,p2, value;

	p1 = (p3x-p2x)*(p1x-p2x) + (p3y-p2y)*(p1y-p2y);
	p2 = (p3x-p2x)*(p1y-p2y) - (p3y-p2y)*(p1x-p2x);

	if (p1 == 0 && p2 == 0) return 0;

	value = atan2(p2,p1);

	return value;
}
inline float angle(float p1x, float p1y, float p2x, float p2y, float p3x, float p3y){
	float p1,p2, value;

	p1 = (p3x-p2x)*(p1x-p2x) + (p3y-p2y)*(p1y-p2y);
	p2 = (p3x-p2x)*(p1y-p2y) - (p3y-p2y)*(p1x-p2x);

	if (p1 == 0 && p2 == 0) return 0;

	value = atan2(p2,p1);

	return value;
}

#define ShowValue(name, value)		{	std::cout<<name<<"\t:\t"<<value<<"\n";	}
#define ShowVector2(name , v2)		{	std::cout<<name<<"\t:\t["<<v2.x<<",\t"<<v2.y<<"]\n";	}
#define ShowVector3(name , v3)		{	std::cout<<name<<"\t:\t["<<v3.x<<",\t"<<v3.y<<",\t"<<v3.z<<"]\n";	}
#define CLIP(x,a,b) __min(__max((x),a),b)
#define Set3Array(x,a,b,c)		{x[0]=a;x[1]=b;x[2]=c;}
#define Set4Array(x,a,b,c,d)	{x[0]=a;x[1]=b;x[2]=c;x[3]=d;}

inline FTP Lerp(FTP f, FTP from, FTP to){ return from + (to - from) * f;}
inline FTP Min(FTP f1, FTP f2){ return f1 < f2 ? f1 : f2;}
inline FTP Max(FTP f1, FTP f2){ return f1 > f2 ? f1 : f2;}
inline FTP Abs(FTP f){return f > 0 ? f : -f;}
inline FTP Sin(FTP f){ return sin(f * ML_DEG_TO_RAD);}
inline FTP Cos(FTP f){ return cos(f * ML_DEG_TO_RAD);}
inline FTP Tan(FTP f){ return tan(f * ML_DEG_TO_RAD);}
inline FTP Asin(FTP f){ return atan(f / sqrt(-f * f + 1)) * ML_RAD_TO_DEG;}			/*[-1,0, 1] --> [-90,0,90]*/
inline FTP Acos(FTP f){ return atan(-f / sqrt(-f * f + 1)) * ML_RAD_TO_DEG + 90.0;}	/*[-1,0, 1] --> [180,90,0]*/
inline FTP AsinR(FTP f){ return atan(f / sqrt(-f * f + 1));} 
inline FTP AcosR(FTP f){ return atan(-f / sqrt(-f * f + 1)) + ML_PI_DIV_2;}
inline FTP Atan(FTP f){ return atan(f) * ML_DEG_TO_RAD;}
inline FTP AtanR(FTP f){ return atan(f);}
inline FTP Rad2Degree(FTP f){ return f * ML_RAD_TO_DEG;}
inline FTP Degree2Rad(FTP f){ return f * ML_DEG_TO_RAD;}
inline FTP Random(){ return (FTP)(rand()) / (FTP)(RAND_MAX);}
inline FTP Random(FTP f){ return Random() * f;}
inline FTP Random(FTP f1, FTP f2){ return Random() * (f2 - f1) + f1;}
inline void SeedRand(unsigned int seed){srand(seed);};

inline bool IsZeroL(FTP f){ return fabs(f) < ML_LOW_TOLERANCE;}
inline bool IsZero(FTP f){ return fabs(f) < ML_TOLERANCE;}
inline bool IsZeroH(FTP f){ return fabs(f) < ML_HIGH_TOLERANCE;}
inline bool IsEqualL(FTP f1, FTP f2){ return fabs(f1 - f2) < ML_LOW_TOLERANCE;}
inline bool IsEqual(FTP f1, FTP f2){ return fabs(f1 - f2) < ML_TOLERANCE;}
inline bool IsEqualH(FTP f1, FTP f2){ return fabs(f1 - f2) < ML_HIGH_TOLERANCE;}

//! simple function to check that a number is a power of two
inline bool isPow2(int v)
{
	int n = 0;
	while (v > 0) {
		if (v & 1) { n ++; }
		v >>= 1;
	}
	return (n == 1);
}

inline bool ValidRange(FTP a, FTP low, FTP up){ return (a>=low&&a<=up);	}
inline bool ValidRange(int a, int low, int up){ return (a>=low&&a<=up);	}
#define AssertRange(r, upper, lower) {	assert(r>=upper && r<=lower);	}

void Solve3x3LinSysDoolittle(FTP a[][3], FTP b[], FTP x[]);
void Solve3x3LinSysGaussElim(FTP a[][3], FTP b[], FTP x[]);

inline int AccumSum(int f1, int f2){  /*sum: f1 + (f1-1) + (f1-2) + ... + f2*/
	int sum = 0;
	if(f1==f2) return f1;
	else if(f1>f2)
		for (int i=f1; i>=f2; --i)		sum+=i;
	else if(f1<f2)
		for (int i=f1; i<=f2; ++i)		sum+=i;
	return sum;
}

inline int AccumMul(int f1, int f2){  /* mul: f1 * (f1-1) * (f1-2) * ... * f2  */
	if(f1<0 || f2<0) return -1;
	int pro = 1;
	if(f1==f2) return f1;
	else if(f1>f2)
		for (int i=f1; i>=f2; --i)		pro*=i;
	else if(f1<f2)
		for (int i=f1; i<=f2; ++i)		pro*=i;
	return pro;
}

inline int MinIdx(FTP f0, FTP f1, FTP f2){
	if(f0<f1){
		if(f0<f2)	return 0;		else		return 2;			
	}
	else{
		if(f1<f2)	return 1;		else		return 2;			
	}
}

inline int MaxIdx(FTP f0, FTP f1, FTP f2){
	if(f0>f1){
		if(f0>f2)	return 0;		else		return 2;			
	}
	else{
		if(f1>f2)	return 1;		else		return 2;			
	}
}

#endif
